//
// Created by jg on 2021/1/6.
//

#ifndef POS_GENERATOR_INSPROBE_TO_POS_H
#define POS_GENERATOR_INSPROBE_TO_POS_H

#include <fstream>
#include <queue>
#include <vector>
#include <memory>
#include <mutex>
/*****  雷达 ******/
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>

#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
/*****  雷达 ******/
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose2D.h>

#include <insprobe_msgs/Navigation.h>
//#include <path_msgs/PathMsg.h>
#include <wmr_msgs/PolynomialTrajectory.h>

#include "pos_generator/oriTrans.hpp"
#include "pos_generator/loc2pos.hpp"
#include "pos_generator/gpstime.h"

using std::vector;
using std::queue;

class INSPROBE_POS{
public:
    enum CTRL_MODE {HANDLE = 0, AUTO, LOOP};

    typedef struct {
        double   unixtime;
        uint16_t week;    /* GPS周 */
        double   weeksec; /* GPS周内秒,s */

        double lat; /* 纬度,deg */
        double lon; /* 经度,deg */
        double alt; /* 椭球高,m */

        double velnorth; /* 北向速度,m/s */
        double veleast;  /* 东向速度,m/s */
        double veldown;  /* 地向速度,m/s */

        double roll;  /* 横滚角,deg */
        double pitch; /* 俯仰角,deg */
        double yaw;   /* 航向角,deg */

        uint8_t navmode; /* 导航模式, 0-纯惯导, 1-SPP松组合, 2-FLOAT松组合, 3-FIXED松组合 */
//    uint8_t flag;    /* 0-END, 1-WITH IMU, 2-WITH GNSS */
//
//    float gyrox;  /* 陀螺X */
//    float gyroy;  /* 陀螺Y */
//    float gyroz;  /* 陀螺Z */
//    float accelx; /* 加表X */
//    float accely; /* 加表Y */
//    float accelz; /* 加表Z */
//
//    double gnsslat;    /* GNSS纬度 */
//    double gnsslon;    /* GNSS经度 */
//    float  gnssalt;    /* GNSS高程 */
//    float  gnsslatstd; /* GNSS纬度标准差 */
//    float  gnsslonstd; /* GNSS经度标准差 */
//    float  gnssaltstd; /* GNSS经度标准差 */
//
//    uint8_t check1;
//    uint8_t check2;
    } InsprobePack;
protected:
    std::ofstream wf_;

    bool has_pos_, has_original_;
    double yaw_last_, time_last_;
    ros::Time   time_now_;
    std::string file_name_;

    //map information
    double resolution_, x_min_, x_max_, y_min_, y_max_;
    //map's original point   lat -deg;  lon -deg;   alt  -m;
    Eigen::Vector3d original_point_;
    //location translation
    LOC2POS::Ptr pos_trans_;

    std::mutex pos_lock_;
    nav_msgs::Odometry state_;
public:
    INSPROBE_POS() : has_pos_(false), has_original_(false){
        yaw_last_  = 0.0;
        time_last_ = 0.0;

        resolution_ = -1.0;
        x_min_ = x_max_ = y_min_ = y_max_ = 0.0;
        pos_trans_ = std::make_shared<LOC2POS>();
    }

    ~INSPROBE_POS(){}

    void init(double resolution, Eigen::Vector3d original, vector<double> map_size);

    bool getLocation(const insprobe_msgs::Navigation& msg, nav_msgs::Odometry& state);

    bool getWayPoint(const geometry_msgs::Pose2D& msg, nav_msgs::Path& wp);

    bool getPathPoint(const Eigen::Vector3d& pos_xyz, geometry_msgs::PoseStamped& pos_BLH);
//    void btraj2path(const wmr_msgs::PolynomialTrajectory& msg, path_msgs::PathMsg& path);


protected:
    std::string double2string(const double value);
};

#endif //POS_GENERATOR_INSPROBE_TO_POS_H
